#include <Servo.h> //调用Servo类
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
//2367
Servo right;  //创建一个舵机控制对象
Servo left; // 使用Servo类最多可以控制8个舵机
MPU6050 mpu;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
float ypr[3];
int a, b, c;

int pos = 0;    // 该变量用与存储舵机角度位置

void setup() 
{ 
  right.attach(9);  // 该舵机由arduino第九脚控制
  left.attach(8);
  
  Serial.begin(115200);//..............................
  mpu.initialize();
  mpu.dmpInitialize();
  mpu.CalibrateAccel(6);
  mpu.CalibrateGyro(6);
  mpu.PrintActiveOffsets();
  mpu.setDMPEnabled(true);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
  pinMode(4,INPUT);
  pinMode(3,INPUT);
  pinMode(2,OUTPUT);
  pinMode(5,INPUT);
  pinMode(6,INPUT);
  pinMode(7,INPUT);
} 

void hd()//跟随活动
{
    right.write(ypr[2] * 60+150 / 3);               // 指定舵机转向的角度
    left.write(ypr[2] * 60+360 / 3);
    delay(60);
}

void sj()//随机立耳(暂时不可用）
{
  for(b=10;b<=60;b++)
  {
    right.write(b);               // 指定舵机转向的角度
    left.write(180-b);
  }
  for(b=60;b>=60;b--)
  {
    right.write(b);               // 指定舵机转向的角度
    left.write(180-b);
  }
}

void zy()//自由活动
{
    for(a=-40;a<=40;a++)
  {
    right.write(a+40);               // 指定舵机转向的角度
    left.write(a+130);
    delay(30);
  }
  for(a=40;a>=-40;a--)
  {
    right.write(a+40);               // 指定舵机转向的角度
    left.write(a+130);
    delay(30);
  }
}

void sd()//手动角度
{
  int xh5 =digitalRead(5);
  int xh6 =digitalRead(6);
  int xh7 =digitalRead(7);
  Serial.print(xh5);
  Serial.print(xh6);
  Serial.print(xh7);
      if(xh5==LOW&&xh6==LOW&&xh7==LOW)
      {
        right.write(10);               // 指定舵机转向的角度
      }
      else if(xh5==LOW&&xh6==HIGH&&xh7==LOW)
      {
        right.write(30 );               // 指定舵机转向的角度
      }
      else if(xh5==LOW&&xh6==LOW&&xh7==HIGH)
      {
        right.write(50 );               // 指定舵机转向的角度
      }
      else if(xh5==HIGH&&xh6==LOW&&xh7==LOW)
      {
        left.write(170 );
      }
      else if(xh5==HIGH&&xh6==HIGH&&xh7==LOW)
      {
        left.write(150 );
      }
      else if(xh5==HIGH&&xh6==LOW&&xh7==HIGH)
      {
        left.write(130 );
      }
    delay(600); 
}

void loop() 
{ 
  digitalWrite(2,HIGH);
  int xh1 =digitalRead(3);
  int xh2 =digitalRead(4);
  if(xh1==LOW&&xh2==LOW)
  {
    hd();
    Serial.print(" \t 跟随活动");
  }
  else if (xh1==LOW&&xh2==HIGH)
  {
    zy();
    Serial.print(" \t 自由活动");
  }
  else if(xh1==HIGH&&xh2==LOW)
  {
    sd();
    Serial.print(" \t 手动角度");
  }
    
   if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))//............................
  {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.print(" \t 摇头");
    Serial.print(ypr[0] * 180 / 3);
    Serial.print(" \t 点头");
    Serial.print(ypr[1] * 180 /3);
    Serial.print(" \t 晃头");
    Serial.println(ypr[2] * 180 / 3);
 }
}
